
#include <stdint.h>
#include <arm_compat.h>

#include "PoolPort.h"
#include "contract.h"
#include "mpool.h"

void MPool_Init(MPool *me, void *poolSto,
                uint32_t poolSize, QMPoolSize blockSize)
{
    QFreeBlock *fb;
    uint32_t corr;
    uint32_t nblocks;

    /* The memory block must be valid
    * and the poolSize must fit at least one free block
    * and the blockSize must not be too close to the top of the dynamic range
    */
    REQUIRE((poolSto != (void *)0)
            && (poolSize >= (uint32_t)sizeof(QFreeBlock))
            && ((QMPoolSize)(blockSize + (QMPoolSize)sizeof(QFreeBlock))
                > blockSize));

    /*lint -e923                    ignore MISRA Rule 45 in this expression */
    corr = ((uint32_t)poolSto & ((uint32_t)sizeof(QFreeBlock) - (uint32_t)1));

    if (corr != (uint32_t)0)                           /* alignment needed? */
    {
        corr = (uint32_t)sizeof(QFreeBlock) - corr;/*amount to align poolSto*/
        poolSize -= corr;                 /* reduce the available pool size */
    }

    /*lint -e826 align the head of free list at the free block-size boundary*/
    me->free = (void *)((uint8_t *)poolSto + corr);

    /* round up the blockSize to fit an integer # free blocks, no division */
    me->blockSize = (QMPoolSize)sizeof(QFreeBlock);  /* start with just one */
    nblocks = (uint32_t)1;    /* # free blocks that fit in one memory block */

    while (me->blockSize < blockSize)
    {
        me->blockSize += (QMPoolSize)sizeof(QFreeBlock);
        ++nblocks;
    }

    blockSize = me->blockSize;      /* use the rounded-up value from now on */

    /* the pool buffer must fit at least one rounded-up block */

    /* chain all blocks together in a free-list... */
    poolSize -= (uint32_t)blockSize;          /* don't count the last block */
    me->nTot  = (QMPoolCtr)1;         /* the last block already in the pool */
    fb        = (QFreeBlock *)me->free;/*start at the head of the free list */

    while (poolSize >= (uint32_t)blockSize)
    {
        fb->next = &fb[nblocks];   /* point the next link to the next block */
        fb = fb->next;                         /* advance to the next block */
        poolSize -= (uint32_t)blockSize;  /* reduce the available pool size */
        ++me->nTot;                /* increment the number of blocks so far */
    }

    fb->next  = (QFreeBlock *)0;            /* the last link points to NULL */
    me->nFree = me->nTot;                            /* all blocks are free */
    me->nMin  = me->nTot;              /* the minimum number of free blocks */
    me->start = poolSto;             /* the original start this pool buffer */
    me->end   = fb;                          /* the last block in this pool */

}


void *MPool_Get(MPool *me)
{
    QFreeBlock *fb;
    PoolCritical_Alloc();

    PoolCritical_Enter();
    fb = (QFreeBlock *)me->free;                /* get a free block or NULL */

    if (fb != (QFreeBlock *)0)                     /* free block available? */
    {
        me->free = fb->next;     /* adjust list head to the next free block */
        --me->nFree;                                 /* one less free block */

        if (me->nMin > me->nFree)
        {
            me->nMin = me->nFree;            /* remember the minimum so far */
        }
    }
    PoolCritical_Exit();
    return fb;            /* return the block or NULL pointer to the caller */
}

void MPool_Put(MPool *me, void *b)
{
    PoolCritical_Alloc();
    REQUIRE((me->start <= b) && (b <= me->end)       /*  must be in range */
            && (me->nFree <= me->nTot)); /* # free blocks must be < total */

    PoolCritical_Enter();
    ((QFreeBlock *)b)->next = (QFreeBlock *)me->free;/* link into free list */
    me->free = b;                       /* set as new head of the free list */
    ++me->nFree;                        /* one more free block in this pool */
    PoolCritical_Exit();
}


